#ifndef __SG_PCL_POINT_FEATURE_CALCULATOR_H__
#define __SG_PCL_POINT_FEATURE_CALCULATOR_H__

#include "SGPCLJsonConfig.h"

#include <pcl/features/fpfh_omp.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/principal_curvatures.h>

#include <vector>

namespace sgpcl
{

/// <summary>
/// 计算点云特征信息的内参数
/// </summary>
struct SGFeatureCalcConfig : public SGPCLJsonConfig
{
  float fRadiusForNormal;    // 计算法向量用到的搜索半径
  float fRadiusForCurvature; // 计算曲率用到的搜索半径

  SGPCL_API SGFeatureCalcConfig();

  SGPCL_API ~SGFeatureCalcConfig() = default;

  /// <summary>
  /// 解析Json字符串，读取参数
  /// </summary>
  /// <param name="sJsonRoot">Json节点</param>
  SGPCL_API void ParseJson(const rapidjson::Value& sJsonRoot) final override;
};

/// <summary>
/// 计算点云法向量
/// </summary>
/// <typeparam name="XYZPoint">坐标点类型</typeparam>
/// <typeparam name="NormPoint">法向量点类型</typeparam>
/// <param name="spXYZCloud">坐标点云</param>
/// <param name="spNormCloud">法向量点云</param>
/// <param name="sConfig">算法内参数</param>
template <typename XYZPoint, typename NormPoint>
void CalculateNorm(typename pcl::PointCloud<XYZPoint>::Ptr spXYZCloud,
  typename pcl::PointCloud<NormPoint>::Ptr spNormCloud, const SGFeatureCalcConfig& sConfig)
{
  // 计时器
  SG_TIMER("计算点云表面外法向量");

  pcl::search::KdTree<XYZPoint>::Ptr spTree(new pcl::search::KdTree<XYZPoint>);
  spTree->setInputCloud(spXYZCloud);

  // 建立法向量估计对象
  pcl::NormalEstimationOMP<XYZPoint, NormPoint> ne;
  ne.setInputCloud(spXYZCloud);
  ne.setSearchMethod(spTree);
  // ne.setRadiusSearch(sConfig.fRadiusForNormal); // 采用半径内的临近点计算法向
  ne.setKSearch(5);

  // 计算法向量
  ne.compute(*spNormCloud);
}

/// <summary>
/// 计算点云曲率
/// </summary>
/// <typeparam name="XYZPoint">坐标点类型</typeparam>
/// <typeparam name="NormPoint">法向量点类型</typeparam>
/// <param name="spXYZCloud">坐标点云</param>
/// <param name="spNormCloud">法向量点云</param>
/// <param name="sConfig">算法内参数</param>
template <typename XYZNormPoint, typename CurvePoint>
void CalculateCurvature(typename pcl::PointCloud<XYZNormPoint>::Ptr spXYZCloud,
  typename pcl::PointCloud<CurvePoint>::Ptr spCurveCloud, const SGFeatureCalcConfig& sConfig)
{

  // 计时器
  SG_TIMER("计算点云曲率");

  pcl::search::KdTree<XYZNormPoint>::Ptr spTree(new pcl::search::KdTree<XYZNormPoint>);
  spTree->setInputCloud(spXYZCloud);

  // 曲率计算
  pcl::PrincipalCurvaturesEstimation<XYZNormPoint, CurvePoint> pce;
  pce.setInputCloud(spXYZCloud);
  pce.setInputNormals(spXYZCloud);
  pce.setSearchMethod(spTree);
  pce.setRadiusSearch(sConfig.fRadiusForCurvature); // 采用半径内的临近点计算曲率

  // 计算最大曲率
  pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr _spCurveCloud(
    new pcl::PointCloud<pcl::PrincipalCurvatures>);
  pce.compute(*_spCurveCloud);

  // 将曲率赋值给点云
  AddCurveToPoint<pcl::PrincipalCurvatures, CurvePoint>(_spCurveCloud, spCurveCloud);
}

template <typename PointT, typename NormT, typename FeatureT>
typename pcl::PointCloud<FeatureT>::Ptr FPFHEstimation(
  typename pcl::PointCloud<PointT>::Ptr spCloud, typename pcl::PointCloud<NormT>::Ptr spNormCloud,
  const SGFeatureCalcConfig& sConfig)
{
  using PointCloud = typename pcl::PointCloud<PointT>;
  using PointCloudPtr = typename pcl::PointCloud<PointT>::Ptr;
  using NormCloud = typename pcl::PointCloud<NormT>;
  using NormCloudPtr = typename pcl::PointCloud<NormT>::Ptr;
  using LocalFeatures = typename pcl::PointCloud<FeatureT>;
  using LocalFeaturesPtr = typename pcl::PointCloud<FeatureT>::Ptr;

  pcl::FPFHEstimationOMP<PointT, NormT, FeatureT> fpfh_est;

  fpfh_est.setInputCloud<PointT>(spCloud);
  fpfh_est.setInputNormals<NormT>(spNormCloud);

  //基于已知的输入数据集，建立kdtree
  pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
  fpfh_est.setSearchMethod(tree);

  // 输出结果
  LocalFeaturesPtr feature(new LocalFeatures);

  //注意：此处使用的半径必须要大于估计表面法线时使用的半径!!!
  fpfh_est.setRadiusSearch(sConfig.fRadiusForNormal);

  //计算获取特征向量
  fpfh_est.compute(*feature);

  return feature;
}

} // !namespace sgpcl

#endif // !__SG_PCL_POINT_FEATURE_CALCULATOR_H__
